import configparser
# 水动力系数结构体
import numpy as np


class MyDynCoe:
	'''
	水动力系数类 保存水动力系数变量
	'''
	rho_w = 1000
	g_e = 9.81
	x_g = 0
	y_g = 0
	z_g = 0
	x_b = 0
	y_b = 0
	z_b = 0.1
	l = 2.38
	m = 167
	ine_xx = 4.73
	ine_yy = 70.27
	ine_zz = 71.38

	# hydrodynamic coefficient
	x_u_ = -4.8e-3
	x_uu = -9.3e-3
	y_r_ = -9.67e-4
	y_v_ = -3.66e-2
	y_r = -4.8e-3
	y_v = -5.69e-2
	y_d_r = -2.15e-2
	z_q_ = -1.86e-4
	z_w_ = -6.8e-2
	z_q = -7.61e-2
	z_w = -8.3e-2

	z_d_s = -3.3e-2
	k_vq = 0
	k_wr = 0
	m_q_ = -4.1e-3
	m_w_ = -1.82e-4
	m_q = -2.07e-2
	m_w = 3.72e-2
	m_d_s = 2.13e-2
	n_r_ = -2.4e-3
	n_v_ = 9.54e-4
	n_r = -9.67e-4
	n_v = -1.86e-2
	n_d_r = 9.9e-3


class APFCoe:
	# 放射斥力系数
	# 误差规则loss-> 反馈规则
	goal_k = 20
	# 手动调参，引力系数 如果仿真超时, 就小幅增加引力
	obj_eta = 1e9
	# 手动调参, 斥力系数 配置迭代方法, 每进行一次仿真计算的所有障碍的最短距离, 如果有小于危险半径的, 累积, 按比例小幅增加斥力, 指导逼近
	obj_r = 2.5 * 10
	# 斥力场范围 取障碍物危险半径的n倍 每进行一次仿真计算, 就统计所有的避障情况, 如果有小于危险半径的, 累积, 增加斥力场范围
	c_obj = 5e-2
	goal = np.zeros(3, dtype=float)
	goal_r = 50
	# 到点半径
	end_dis = 6


class ObjectInfo:
	x = np.zeros(6, dtype=float)
	nu = np.zeros(6, dtype=float)
	lhw = np.zeros(3, dtype=float)
	danger_r = 30
	type = 'none'


class ControlParams:
	# 船体状态和驱动
	x = np.zeros(6, dtype=float)
	u = np.zeros(6, dtype=float)
	nu = np.zeros(6, dtype=float)
	f = np.zeros(6, dtype=float)
	delta = np.zeros(3, dtype=float)
	# 期望速度
	target_u = 3
	# 减速速度
	low_u = 1
	# 增量PID控制用
	pidu = np.zeros(3, dtype=float)
	oldedx = np.zeros(3, dtype=float)
	old2edx = np.zeros(3, dtype=float)
	# 跟踪任务完成情况 0--False 1--True
	flag = 0
	# 任务模式 1--跟踪  2--避障
	mode = 1
	# 驱动方式 1--舵机驱动 2--差速驱动
	f_mode = 1


# 自动迭代参数
filename = './ini/set.ini'
# 外部手动修改参数
extern_file = './ini/refer.ini'
# 外部手动修改的水动力参数
boat_file = './ini/smallBoat.ini'
apf_section = 'APFCoe'


def ini_read(name):
	'''
	读取配置文件并返回若干结构体
	:param name: 字符串 配置文件名
	:return: APFCoe类型
	'''
	config = configparser.ConfigParser()
	config.read(name)
	coe = APFCoe()
	coe.goal_k = config.get(apf_section, 'goal_k')
	coe.obj_eta = config.get(apf_section, 'obj_eta')
	coe.obj_r = config.get(apf_section, 'obj_r')
	coe.c_obj = config.get(apf_section, 'c_obj')
	return coe


def ini_write(name, coe):
	config = configparser.ConfigParser()
	config.read(name)
	if config.has_section(apf_section):
		print(apf_section, ' has exist')
	else:
		config.add_section(apf_section)
	config.set(apf_section, 'goal_k', str(coe.goal_k))
	config.set(apf_section, 'obj_eta', str(coe.obj_eta))
	config.set(apf_section, 'obj_r', str(coe.obj_r))
	config.set(apf_section, 'c_obj', str(coe.c_obj))

	# 写入配置文件
	config.write(open(filename, 'w'))

# -------------------------------------TEST--------------------------------
# cc = APFCoe()
# ini_write(filename, cc)
